#include <Wire.h>
#include "JY901.h"
#include <SoftwareSerial.h>
#include "ClickButton.h"
#include "MouseKA8.h"
#define DEBUG_ONLINE


#define GREEN  9
#define YELL1  10
#define YELL2  11
#define RED    12
#define BUZZER 7
#define VOLTAGE A0
#define BTN1	6
#define BTN2	8
#define USBPLUGIN A6
#define SEPRATE A5
#define CONNECT_STATE 4

#define RF_TX A1
#define RF_RX A2
#define RF_DI A3
#define RF_SL A4

#define MOUSE_SCL 3
#define MOUSE_SDA 2

#define MAJOR_VERSION 1
#define MINOR_VERSION 1
#define HEAD 0x50
#define SHOW 0xAA
#define WARNING 0xFF
#define FIXTIME 200

// more than 3.98  full ----------------4
// between 3.98 and 3.77----------------3
// betwn 3.77 and 3.68------------------2
// less than 3.68-----------------------1
#define FULL 3.98 // more than 4 stars
#define HALF  3.77//less than 3 stars
#define LIMIT_VOLTAGE 3.68
ClickButton button1(BTN1, LOW, CLICKBTN_PULLUP);
SoftwareSerial mySerial(RF_RX, RF_TX); // RX, TX
MouseKA8 MKA(MOUSE_SDA,MOUSE_SCL);
byte matrix[6][5]{
	{0xf3,0xaa,0x01,0x01,0x00},
	{0xf3,0xaa,0x01,0x00,0x00},
	{0xf3,0xaa,0x00,0x00,0x00},
	{0xf3,0xaa,0x01,0x07,0x00},
	{0xf3,0xaa,0x01,0x00,0x00},
	{0xf3,0xaa,0x00,0x00,0x00}
};

int codeWarn;
int buttonState; 
long timerSend2RF;
int debounceDelay =50;
unsigned long lastDebounceTime;
byte lastEventBtn;
struct data2RF
{
	byte head;
	byte yaw_Hi;
	byte yaw_Lo;
	byte Pitch_Hi;
	byte Pitch_Lo;
	byte Roll_Hi;
	byte Roll_Lo;
	byte keys;
};
struct data2RF d2rf;
int offLedInterval;
boolean isShowVoltage = false;
int index = 0;
void init_io()
{
	// pinMode(MOUSE_SCL,OUTPUT);
	pinMode(RF_DI,INPUT_PULLUP);
	pinMode(RF_SL,INPUT_PULLUP);
	pinMode(CONNECT_STATE,OUTPUT);
	pinMode(BTN1,INPUT_PULLUP);
	pinMode(BTN2,INPUT_PULLUP);
	pinMode(USBPLUGIN,INPUT);
	pinMode(SEPRATE,INPUT_PULLUP);
	pinMode(BUZZER,OUTPUT);
	pinMode(GREEN, OUTPUT);
	pinMode(YELL1, OUTPUT);
	pinMode(YELL2, OUTPUT);
	pinMode(RED, OUTPUT);

}
void setup() {
  // put your setup code here, to run once:
  init_io();
  Serial.begin(9600);
  mySerial.begin(9600);
  buzzerVersion();
  MKA.init_reg();
}

void loop() {


	button1.Update();
	byte btn1 = digitalRead(BTN1);
	byte btn2 = digitalRead(BTN2);
	int usbPlug = analogRead(USBPLUGIN);
	boolean isRfBusy = digitalRead(RF_DI)==LOW?true:false;
	int vol = analogRead(VOLTAGE);
	byte spe = digitalRead(SEPRATE)==HIGH?HIGH:LOW;

	//send data to RF while SWITCH CLOSED;
	if(millis()-timerSend2RF>=20)//&&isRfBusy)
	{
		timerSend2RF = millis();
		d2rf = formatDate(HEAD,btn1,btn2);
		byte *toByte = (byte*)&d2rf;
		mySerial.write(toByte,sizeof(data2RF));
		if (isShowVoltage)
		{
			offLedInterval++;
		}
		if (offLedInterval%200==0)
		{
			allOffLed();
			isShowVoltage = false;
		}
	}

	//show current voltage 
	voltageDisplay(btn2,usbPlug,vol);

	//calibration the 9-axis sensor
	if (spe == HIGH)
	{
		byte toSend[5];

		if (button1.changed)
		{
			virbration(index+1);
			//Serial.println("hei");
			for (int i = 0; i < 6; ++i)
			{
				toSend[i] = matrix[index][i];
			}
			Serial.write(toSend,5);
			index++;
			if (index>5)
			{
				index = 0;
			}
		}
	}

	//mouse data display
	MKA.position();

}
void virbration(int times)
{
	for (int i = 0; i < times; ++i)
	{
		digitalWrite(BUZZER,HIGH);
		delay(50);
		digitalWrite(BUZZER,LOW);
		delay(50);
	}
}
void voltageDisplay(byte evetBtn,int eventUsb,int eventlowBatt)
{

	float voltageTemp = ((eventlowBatt/1024.0)*3.3)*2;
	if (evetBtn!=lastEventBtn)
	{
		lastDebounceTime = millis();
	}

	if (millis()-lastDebounceTime>debounceDelay)
	{
		if (evetBtn!=buttonState)
		{
			buttonState = evetBtn;
			if (buttonState==LOW)
			{
				isShowVoltage = true;
				displayWay(SHOW,voltageTemp);
				//LOGln(7788);
			}
		}
	}

	if (voltageTemp<LIMIT_VOLTAGE)
	{
		codeWarn++;
		if (codeWarn>=20)
		{
			displayWay(WARNING,voltageTemp);
		}
	}
	else
	codeWarn = 0;
	lastEventBtn = evetBtn;

}
void displayWay(byte type,float voltage)
{
	if (type == SHOW)
	{
		Serial.println(voltage);
		if (voltage>FULL)//----------------------------------4
		{
			for (int i = GREEN; i <= RED; ++i)
			{
				digitalWrite(i,HIGH);
			}
		}
		if (voltage>HALF&&voltage<FULL)//---------------3
		{
			for (int i = YELL1; i <= RED; ++i)
			{
				digitalWrite(i,HIGH);
			}
		}
		if (voltage<HALF&&voltage>LIMIT_VOLTAGE)//-----------2
		{
			for (int i = YELL2; i <= RED; ++i)
			{
				digitalWrite(i,HIGH);
			}
		}

	}
	if (type == WARNING)
	{
		for (int i = 0; i < 3; ++i)
		{
			digitalWrite(RED,LOW);
			delay(FIXTIME);
			digitalWrite(RED,HIGH);
			delay(FIXTIME);
		}
	}
}
struct data2RF formatDate(byte lead,byte key1,byte key2)
{
	struct data2RF temp;
	temp.head = lead;
	temp.yaw_Hi = JY901.stcAngle.Angle[0] >> 8;
	temp.yaw_Lo = JY901.stcAngle.Angle[0];
	temp.Pitch_Hi = JY901.stcAngle.Angle[1] >> 8;
	temp.Pitch_Lo = JY901.stcAngle.Angle[1];
	temp.Roll_Hi = JY901.stcAngle.Angle[2] >> 8;
	temp.Roll_Lo = JY901.stcAngle.Angle[2];
	temp.keys = (key1&0xff)<<1|(key2&0xff);
	return temp;
}
/*
  SerialEvent occurs whenever a new data comes in the
  hardware serial RX.  This routine is run between each
  time loop() runs, so using delay inside loop can delay
  response.  Multiple bytes of data may be available.
  */
  void serialEvent()
  {
  	while (Serial.available())
  	{
    JY901.CopeSerialData(Serial.read()); //Call JY901 data cope function
}
}
void buzzerVersion(void)
{
	#if defined(BUZZER)
    // flash the pin with the protocol version
    strobeBuzzerPin(BUZZER, MAJOR_VERSION, 40, 210);
    delay(250);
    strobeBuzzerPin(BUZZER, MINOR_VERSION, 40, 210);
    delay(125);
    #endif
}
void strobeBuzzerPin(byte pin, int count, int onInterval, int offInterval)
{
	byte i;
	for (i = 0; i < count; i++)
	{
		delay(offInterval);
		digitalWrite(pin, HIGH);
		delay(onInterval);
		digitalWrite(pin, LOW);
	}
}
void LOG(int x)
{
	#ifdef DEBUG_ONLINE
	Serial.print(x);
	#endif
}
void LOGfln(float x)
{
	#ifdef DEBUG_ONLINE
	Serial.println(x);
	#endif
}
void LOGln(int x)
{
	#ifdef DEBUG_ONLINE
	Serial.println(x);
	#endif
}
void allOffLed()
{
	for (int i = GREEN; i <= RED; ++i)
	{
		digitalWrite(i,LOW);
	}
}
 